import gradio as gr
import os
import datetime
import time
from myMotor import StepperMotor
import serial
import RPi.GPIO as GPIO
import json
import drag_trial
from drag_trial import TeachingTest

if os.name == "nt":
    import keyboard
    import webbrowser
    show_app = True
else:
    show_app = False

saved_path = "./saved"
action_group_path = "./action_groups"
data_id = [34, 36]  #34保护扭矩 36过载扭矩

zero_setted = 0
init_flag = 0
AXIS_MAX_POSITION = 0
#正方向为朝向电机方向
mserial = serial.Serial('/dev/ttyAMA1', 115200, timeout=0.5)
testMotor = StepperMotor(mserial, 0x01)

#创建文件夹
if not os.path.exists(saved_path):
    os.makedirs(saved_path)
if not os.path.exists(action_group_path):
    os.makedirs(action_group_path)

temp_action_group = []
saved_actions = []
for filename in os.listdir(saved_path):
    file_path = os.path.join(saved_path, filename)
    if os.path.isfile(file_path):
        saved_actions.append(filename.split(".")[0])
saved_actions.append("夹取")
saved_actions.append("释放")

saved_action_groups = []

gripper_state_flag = 0

for filename in os.listdir(action_group_path):
    file_path = os.path.join(action_group_path, filename)
    if os.path.isfile(file_path):
        saved_action_groups.append(filename.split(".")[0])


def robotControl(action, name):
    global recorder
    if action == "录制机械臂动作":
        recorder.mc.release_all_servos()
        recorder.record()
        return
    elif action == "停止录制":
        recorder.stop_record()
        return
    elif action == "播放单动作":
        recorder.play()
        return
    elif action == "保存动作":
        if name == "default":
            # 获取当前的日期时间
            current_time = datetime.datetime.now()
            # 将日期时间转换为秒级时间戳
            timestamp = current_time.strftime("%Y%m%d%H%M%S")
            # 构建文件名
            filename = f"action_{timestamp}"
        else:
            filename = name
        recorder.save_to_local(saved_path + "/" + filename + ".txt")
        return
    elif action == "释放机械臂关节":
        recorder.mc.release_all_servos()
        return
    elif action == "退出":
        if show_app:
            keyboard.press_and_release("Ctrl+w")
        os._exit(0)


def gripperControl(value: str = ""):
    global recorder
    if value == "":
        global gripper_state_flag
        recorder.mc.set_gripper_state(gripper_state_flag, 70)
        gripper_state_flag = 1 - gripper_state_flag
    else:
        #value转int
        recorder.mc.set_encoder(7, int(value))
    time.sleep(1)
    return


def load1action(folder_path, action_name):
    if action_name == "":
        return "need 1 action"
    elif action_name == "夹取":
        gripperControl("1700")
        time.sleep(1)
        return
    elif action_name == "释放":
        gripperControl("2048")
        time.sleep(1)
        return
    recorder.load_from_local(folder_path + "/" + action_name)


def view_actions(folder_path):
    saved_actions = []
    for filename in os.listdir(folder_path):
        file_path = os.path.join(folder_path, filename)
        if os.path.isfile(file_path):
            saved_actions.append(filename.split(".")[0])
    saved_actions.append("夹取")
    saved_actions.append("释放")
    new_goup = gr.Dropdown.update(choices=saved_actions)
    return new_goup, new_goup


def view_action_groups(folder_path, action_group):
    global temp_action_group
    action_groups = []
    for filename in os.listdir(folder_path):
        file_path = os.path.join(folder_path, filename)
        if os.path.isfile(file_path):
            action_groups.append(filename.split(".")[0])
    new_goup = gr.Dropdown.update(choices=action_groups)
    mystr = ""
    selected_group = gr.Markdown.update(mystr)
    if action_group == "":
        return new_goup, selected_group
    #读取action_group对应的json文件，将其中的动作顺序读取出来，然后生成markdown格式的字符串
    json_file = open(folder_path + "/" + action_group + ".json", "r")
    data = json.load(json_file)
    for i in range(len(data["action_group"])):
        mystr += str(i + 1) + ". " + data["action_group"][i] + "\n"
    selected_group = gr.Markdown.update(mystr)
    temp_action_group = data["action_group"]
    return new_goup, selected_group

def play_action_group():
    global temp_action_group,saved_path
    for action in temp_action_group:
        load1action(saved_path, action)
        if action not in ["夹取", "释放"]:
            recorder.play()
    return "动作组播放完毕"

def add_action2group(action):
    #根据action dropdown选项添加新的action到动作组，生成格式为markdown，标记好动作次序
    #然后将其作为输出绑定到markdown组件
    global temp_action_group
    temp_action_group.append(action)
    mystr = ""
    for i in range(len(temp_action_group)):
        mystr += str(i + 1) + ". " + temp_action_group[i] + "\n"
    new_group = gr.Markdown.update(mystr)
    return new_group


def delete_action2group():
    global temp_action_group
    mystr = ""
    if len(temp_action_group) == 0:
        new_group = gr.Markdown.update(mystr)
        return new_group
    temp_action_group.pop()
    for i in range(len(temp_action_group)):
        mystr += str(i + 1) + ". " + temp_action_group[i] + "\n"
    new_group = gr.Markdown.update(mystr)
    return new_group


def save_action_group(name, path):
    global temp_action_group
    #将temp_action_group列表保存为json文件
    #内容包括动作组数量，动作名称，动作顺序
    #文件名为name.json
    #保存路径为path
    json_file = open(path + "/" + name + ".json", "w+")
    data = {"action_group": []}
    data["action_group"] = temp_action_group
    json.dump(data, json_file, indent=4)
    return


def read_pid():
    global data_id
    data_log = ''
    for i in range(1, 8):
        for j in range(len(data_id)):
            data_log = data_log + "电机 " + str(i) + (
                " 保护扭矩:" if data_id[j] == 34 else " 过载扭矩:") + " : " + str(
                    recorder.mc.get_servo_data(i, data_id[j])) + "  \n"
    print(data_log)
    return data_log


def write_pid(motor, torque_class, value):
    if torque_class == "保护扭矩":
        d_id = 34
    elif torque_class == "过载扭矩":
        d_id = 36
    else:
        print("wrong torque class")
        return
    recorder.mc.set_servo_data(int(motor), d_id, int(value))
    return

def set_axis_value(value):
    global AXIS_MAX_POSITION,testMotor
    if value > AXIS_MAX_POSITION:
        value = AXIS_MAX_POSITION
        print("超出最大位置，已设置为最大位置"+str(AXIS_MAX_POSITION))
    elif value < 0:
        value = 0
        print("超出最小位置，已设置为最小位置")
    testMotor.position_mode_control_smooth(position=int(value*10), relative_position=False)
    time.sleep(0.1)
    while testMotor.read_motor_position_error()>0.1:
        time.sleep(0.1)
    return

with gr.Blocks() as demo:
    gr.Markdown("机械臂控制面板")
    with gr.Row():
        folder_input = gr.Textbox(label="保存的动作路径",
                                  value=saved_path)
        load_button = gr.Button("刷新保存的动作")
    with gr.Tab("指令"):
        action = gr.Radio(
            ["录制机械臂动作", "停止录制", "播放单动作", "保存动作", "释放机械臂关节", "退出"],
            label="指令",
            info="选择指令")
        save_name = gr.Textbox(label="动作名称",
                               value="default",
                               info="输入保存的动作名称,默认会按照当前时间保存")
        action_button = gr.Button("执行")
        with gr.Row():
            one_action = gr.Dropdown(choices=saved_actions, label="动作")
            load_one_action = gr.Button("读取单动作")
        with gr.Row():
            axis_value=gr.Slider(minimum=0,maximum=5400,value=0,label="第七轴电机角度")
            set_axis=gr.Button("设置第七轴电机角度")
        with gr.Row():
            gripper_value = gr.Slider(minimum=1450,
                                      maximum=2048,
                                      value=2048,
                                      label="夹爪值",
                                      info="输入夹爪的值1000-2048(关闭-释放)")
        gripper_set = gr.Button("设置夹爪值")
        gripper_state_set = gr.Button("切换夹爪状态(开/关)")
    with gr.Tab("动作组"):
        action_groups = gr.Dropdown(choices=saved_action_groups,
                                            label="保存的动作组")
        with gr.Row():
            load_button2 = gr.Button("加载并刷新动作组")
            play_group = gr.Button("启动动作组")
        #使用Dropdown动态读取文件夹下的文件名，并将其作为选项列出，并且根据勾选生成一个列表
        with gr.Row():
            actions = gr.Dropdown(choices=saved_actions, label="保存的动作")
            #action_group=gr.CheckboxGroup(choices=saved_actions,label="Actions")
            add_action = gr.Button("添加一个动作")
            delete_action = gr.Button("删除最后的动作")
        with gr.Row():
            with gr.Column():
                temp_action_group_label = gr.Label("当前动作组")
                temp_group = gr.Markdown("...")
            with gr.Column():
                group_name = gr.Textbox(label="动作组名称",
                                        value="action_group1")
                folder_action_group = gr.Textbox(
                    label="动作组保存路径",
                    value=action_group_path)
                save_group = gr.Button("保存当前动作组")
    with gr.Tab("电机扭矩设置"):
        with gr.Row():
            motor = gr.Dropdown(choices=["1", "2", "3", "4", "5", "6", "7"],
                                label="选择电机")
            torque_class = gr.Dropdown(choices=["保护扭矩", "过载扭矩"],
                                       label="设置扭矩类型")
            torque = gr.Textbox(label="设置扭矩值", value="0")
            write_pid_button = gr.Button("写入设置")
        read_pid_button = gr.Button("读取设置")

    log = gr.Markdown("...")

    action_button.click(fn=robotControl,
                        inputs=[action, save_name],
                        outputs=None)
    load_button.click(fn=view_actions,
                      inputs=folder_input,
                      outputs=[actions, one_action])
    #actions.change(fn=change_action_group, inputs=actions, outputs=temp_group)
    #action_group.change(fn=change_action_group, inputs=action_group, outputs=temp_group)
    load_one_action.click(fn=load1action,
                          inputs=[folder_input, one_action],
                          outputs=None)
    set_axis.click(fn=set_axis_value, inputs=axis_value, outputs=None)
    
    play_group.click(fn=play_action_group,inputs=None,outputs=log)
    add_action.click(fn=add_action2group, inputs=actions, outputs=temp_group)
    delete_action.click(fn=delete_action2group,
                        inputs=None,
                        outputs=temp_group)
    save_group.click(fn=save_action_group,
                     inputs=[group_name, folder_action_group],
                     outputs=log)
    load_button2.click(fn=view_action_groups,
                       inputs=[folder_action_group, action_groups],
                       outputs=[action_groups, temp_group])

    gripper_set.click(fn=gripperControl, inputs=gripper_value, outputs=None)
    gripper_state_set.click(fn=gripperControl, inputs=None, outputs=None)
    read_pid_button.click(fn=read_pid, inputs=None, outputs=log)
    write_pid_button.click(fn=write_pid,
                           inputs=[motor, torque_class, torque],
                           outputs=None)

def init_axis_zero(channel, motor: StepperMotor):
    """
    @description  :初始化轴零点，设为光电门正方向180度电机位置
    ---------
    @param  :motor:电机对象,init_flag:初始化标志位(全局变量)
    -------
    """
    global zero_setted, init_flag
    motor.stop()#stop执行后电机不会刷新当前位置，需要重新读取
    if (init_flag == 0):
        time.sleep(0.1)
        motor.set_zero_position()
        time.sleep(0.1)
        motor.position_mode_control_smooth(position=1800,max_speed=1000)
        time.sleep(0.1)
        while motor.read_motor_position_error() > 0.1:
            time.sleep(0.1) #等待电机到达指定位置
        print("初始化轴零点")
        motor.set_zero_position()
        zero_setted = 1
    else:
        print("软件限位与硬件限位冲突")
        exit(1)

    return None


def init_axis_max(channel, motor: StepperMotor):
    """
    @description  :初始化轴最大位置，设为光电门负方向180度电机位置
    ---------
    @param  :motor:电机对象,init_flag:初始化标志位(全局变量),
                AXIS_MAX_POSITION:轴最大位置(全局变量)
    -------
    """
    global AXIS_MAX_POSITION, init_flag
    motor.stop()
    if (init_flag == 0):
        time.sleep(0.1)
        print("初始化轴最大位置")
        AXIS_MAX_POSITION = motor.read_motor_realtime_position() - 180.0
        init_flag = 1
    else:
        print("软件限位与硬件限位冲突")
        exit(1)
    return None

if __name__ == "__main__":
    drag_trial.setup()
    recorder = TeachingTest(drag_trial.mc)

    print(testMotor.read_firmware_and_hardware_version())

    # 设置GPIO模式为BCM
    GPIO.setmode(GPIO.BCM)
    # 设置BCM 17引脚为输入模式，并启用上拉电阻（连接电机远端光电门）
    GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    # 设置BCM 18引脚为输入模式，并启用上拉电阻（连接电机近端光电门）
    GPIO.setup(18, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    init_axis_zero_with_param = lambda channel: init_axis_zero(
        channel, testMotor)
    init_axis_max_with_param = lambda channel: init_axis_max(
        channel, testMotor)
    GPIO.add_event_detect(17,
                          GPIO.RISING,
                          callback=init_axis_zero_with_param,
                          bouncetime=50)
    GPIO.add_event_detect(18,
                          GPIO.RISING,
                          callback=init_axis_max_with_param,
                          bouncetime=50)
    #归零校准
    init_max_speed = 1000
    gate_far = GPIO.input(17)
    gate_near = GPIO.input(18)
    if (gate_far):  #向正方向前进360度
        testMotor.position_mode_control_smooth(position=3600,
                                               max_speed=init_max_speed)
    if (gate_near):  #向负方向前进360度
        testMotor.position_mode_control_smooth(position=-3600,
                                               max_speed=init_max_speed)
    testMotor.speed_mode_control(speed=-500,
                                 slope=1000)  #先向负方向运动直到触发远端光电门并设置零点
    while (zero_setted == 0):
        time.sleep(0.1)
    testMotor.speed_mode_control(speed=500,
                                 slope=1000)  #再向正方向运动直到触发近端光电门并设置最大位置
    while (init_flag == 0):
        time.sleep(0.1)
    testMotor.position_mode_control_smooth(position=0,relative_position=False)
    
    if show_app:
        webbrowser.open_new_tab("http://localhost:7860")
    demo.launch(server_name="0.0.0.0")
